﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using ROSBridgeLib;
using ROSBridgeLib.nav_msgs;
using ROSBridgeLib.geometry_msgs;
using UnityEngine.UI;

public class RosConnector : MonoBehaviour
{



    public string ip = "192.168.1.38";

    public int port = 9090;

    private ROSBridgeWebSocketConnection conn;


    public Map2D map2d;


    public Transform robot;

    public Transform target;


    public LineRenderer navPath;


    public Frame[] frames;


    public Hashtable frameTransforms;


    [SerializeField] Text displayMessage;


    [SerializeField] Text txtBtnConnect;

    private bool connected=false;


    // Use this for initialization
    void Start()
    {

        InitTransforms();
      
    }

    void InitTransforms()
    {

       

        frameTransforms=new Hashtable();

        frames=FindObjectsOfType<Frame>() as Frame[];

        foreach (Frame frame in frames)
        {

            frameTransforms.Add(frame.frame_id,frame.transform);

        }


    }


    public void OnBtnConnectClick()
    {


        if (connected)
        {
            Disconnect();
        }else
        {
            Connect();

        }

    }
    public void OnConnect()
    {

        displayMessage.text="Connected";


        connected=true;

    }


    public void OnError()
    {

        connected=false;

       


    }

    public void OnDisconnect()
    {


        connected=false;



    }
	


   
    void Update()
    {
		
      

        if (conn != null)
        {
            
            conn.Render();


            if (connected)
            {
                txtBtnConnect.text="断开连接";

            }else
            {
                txtBtnConnect.text="连接";
            }

        }


    }


    public void Connect()
    {

        conn = new ROSBridgeWebSocketConnection("ws://" + ip, port);

        conn.AddSubscriber(typeof(MapSuscribler));

//        conn.AddSubscriber(typeof(RobotPoseSubscribiler));

        conn.AddSubscriber(typeof(NavPathSubscriber));

        conn.AddSubscriber(typeof(TransformsSubscriber));
       
        conn.AddPublisher(typeof(NavGoalPublisher));

        conn.onConnnect+=OnConnect;

        conn.onError+=OnError;

        conn.onDisconnect+=OnDisconnect;

        conn.Connect();



    }





    public void Disconnect()
    {
        if (conn != null)
        {
            conn.Disconnect();

        }

    }



    public void SetNavGoal(Vector3 position,Quaternion rotation)
    {

       

        if (conn!=null)
        {

            PoseStampedMsg msg=new PoseStampedMsg();

            msg.pose.position.FromVector3(position);
            msg.pose.orientation.FromQuaternion(rotation);
            msg.header._frame_id="/map";


//            conn.Publish (NavGoalPublisher.GetMessageTopic(), msg);


            target.localPosition=msg.pose.position.ToVector3();
            target.localRotation=rotation;
        }
    }

    public void OnGetRobotPoseStamped(PoseStampedMsg poseStamped)
    {
        


//        if (poseStamped != null)
//        {
//          
//            robot.localPosition=poseStamped.pose.position.ToVector3();
//          
//            robot.localRotation=poseStamped.pose.orientation.ToQuaternion();
//
//           
//        }


    }

    public void OnGetMap(OccupancyGrid map)
    {
        if (map != null)
        {
            map2d.MapDisplay(map);
        }
    }

    public bool isAlive()
    {

        if (conn != null)
        {

            return  conn.isAlive();

        }

        return false;

    }

    void OnApplicationQuit()
    {

       
        Disconnect();

    }


    public void OnNavPathReceived(PathMsg msg)
    {


        navPath.numPositions=msg.poses.Length;

        for (int i=0;i<msg.poses.Length;i++)
        {
            
            navPath.SetPosition(i,msg.poses[i].pose.position.ToVector3());

        }


    }


    public void OnGetTranforms(TransformsMsg transforms)
    {



        foreach(TransformMsg tran in transforms.transforms)
        {



            Transform parent=(Transform)frameTransforms[tran.frameId];
            Transform child=frameTransforms[tran.childFrameId] as Transform;


            if (parent!=null && child!=null)
            {

                child.position=parent.TransformPoint(tran.translation.ToVector3());


                child.rotation=parent.rotation*tran.rotation.ToQuaternion();


            }



        }


    }




}
